import pyb
'''
maincontrol:
    1.定义基础控制的常量，最大速度和最小速度
    2.PS2手柄控制的逻辑代码
    3.自动控制的逻辑代码

control函数：
    函数功能：根据数据处理获得电机的速度
    入口参数：控制机构数据/上位机数据
    返回值  ：电机速度(0-max_speed)
driver函数：
    函数功能：根据电机速度，控制电机运动方向，同时进行pid运算得到电机pwm值
    入口参数： 电机速度
    返回值  ：电机pwm(0-100)
'''
max_speed = 3  # 电机可达到的最大速度,手动测出
min_speed = 0
max_range = 100 * max_speed # 最大速度*100的范围
'''
    最大速度为3，PWM为90-100
    速度    PWM占空比
    0.5 —— 15.28
    '''

class Ps2_Driver:
    
    def __init__(self, encoder, pid, motor):
        self.enc = encoder
        self.pid = pid
        self.motor = motor
        
    def ps2_driver_init(self):
        # 赋初值
        self.speed = 0

    def ps2_joycontrol(self, joy_data):
        '''
        函数功能：PS2手柄的摇杆控制,无级调速的前进、后退、差速左转、差速右转、原地左转和原地右转
        入口参数：ps2手柄摇杆值列表
        返回值 ：PS2手柄摇杆读取到的数值对应的速度
        '''
        global ahead_flag, back_flag, left_flag, right_flag
        global max_speed, min_speed, max_range
        
        angle_speed = (joy_data[5] - 192) * 255 / (192-128) * max_speed # 换算成-100-100 * 速度最大值 = +-速度范围* 100
        speed =  (joy_data[8] - 191) * 255 / (191-128) * max_speed  # 换算成-100-100 * 速度最大值 = +-速度范围* 100
        # 方向判断
        if speed < 0:
            speed = abs(speed)
            ahead_flag = True 
            back_flag = False
        elif speed > 0:
            ahead_flag = False 
            back_flag= True 
        else:
            speed = 0
            ahead_flag = False 
            back_flag = False
        # 左右转判断
        if angle_speed > 0: # 右转
            speed_A = speed + angle_speed; speed_B = abs(speed - angle_speed); left_flag = False; right_flag = True;# 保证速度值都为正，正负只用做判断方向确定flag
        elif angle_speed < 0: # 左转为负
            speed_A = abs(speed - abs(angle_speed)); speed_B = speed + abs(angle_speed); left_flag = True; right_flag = False; 
        else:
            speed_A = speed; speed_B = speed; left_flag = False; right_flag = False;
        # 速度限幅
        speed_A, speed_B = self.speed_limit(max_range, speed_A, speed_B)
        return speed_A/100, speed_B/100


    def ps2_keycontrol(self, key):
        '''
        函数功能：使用PS2手柄调节速度值，用于测试PID值，调参使用
        入口参数：按键值
        返回值  ：电机速度值
        '''
        global ahead_flag, back_flag, left_flag, right_flag
        global max_range
        # print(key)
        if key == 5: # 上
            self.speed += 10
        elif key == 7: # 下
            self.speed -= 10
        elif key == 12: # 右前方按钮，停止
            self.speed = 0
        elif key == 11: # 左前方按钮，重启
            pyb.hard_reset()
        else:
            if self.speed != 0:
                self.speed = self.speed
            else:
                self.speed = 0
        # 方向判断
        if self.speed < 0:
            abs_speed = abs(self.speed)
            ahead_flag = True; back_flag = False
        elif self.speed > 0:
            ahead_flag = False; back_flag= True
            abs_speed = self.speed
        else:
            abs_speed = 0
            ahead_flag = False; back_flag = False
        left_flag = False; right_flag = False
        if self.speed == 0:
            return 0, 0
        else:
            speed_A, speed_B = self.speed_limit(max_range, abs_speed, abs_speed)
            return speed_A/100, speed_B/100

    def speed_limit(self, max_range, speed_A, speed_B):
        '''
        函数功能：速度限幅
        入口参数：输入速度
        返回值 ：输出的双轮的运动速度
        '''
        if (speed_A > max_range):
            speed_A = max_range
        elif (speed_A < 0): 
            speed_A = 0
        if (speed_B > max_range): 
            speed_B = max_range
        elif (speed_B < 0): 
            speed_B = 0
        return speed_A, speed_B

    def driver(self, target_enc_A, target_enc_B, cur_enc_A, cur_enc_B):
        '''
        函数功能：PS2手柄运动控制
        入口参数：目标双轮编码器值，当前双轮编码器值
        返回值  ：无
        '''
        global ahead_flag, back_flag, left_flag, right_flag
        
        pwm_A = self.pid.incremental_pid(cur_enc_A, target_enc_A) 
        pwm_B = self.pid.incremental_pid(cur_enc_B, target_enc_B) 
        self.motor.motor_run(pwm_A, pwm_B)
        self.motor.direction_control(ahead_flag, back_flag, left_flag, right_flag)


class Auto_Driver:
    def __init__(self, encoder, pid, motor):
        self.enc = encoder
        self.pid = pid
        self.motor = motor

    def auto_control(self, ser_msg):
        pass
